#pragma once
#include <cmath>

/**
 * Vehicle enu坐标系
 * @param x     正东
 * @param y     正北
 * @param theta 航向角 从东方向逆时针旋转到车头朝向的角度（弧度，东方向为0，逆时针增加）
 * @param alpha 目标偏离角（弧度，顺时针为正）
 */
struct Vehicle
{
    double x;     // 车辆东向坐标（米）
    double y;     // 车辆北向坐标（米）
    double theta; // 航向角（弧度，东方向为0，逆时针增加）
    double alpha; // 目标偏离角（弧度，顺时针为正）
    Vehicle(double x_, double y_, double the_, double alp_) : x(x_), y(y_), theta(the_), alpha(alp_) {}
};

/**
 * VehicleWgs84坐标系
 * @param longitude 经度 --> x
 * @param latitude  纬度 --> y
 * @param theta     航向角（弧度，东方向为0，逆时针增加）
 * @param alpha     目标偏离角（弧度，顺时针为正）
 */
struct VehicleWgs84
{
    double longitude; // 经度 --> x
    double latitude;  // 纬度 --> y
    double theta;     // 航向角（弧度，东方向为0，逆时针增加）
    double alpha;     // 目标偏离角（弧度，顺时针为正）
    VehicleWgs84(double lon, double lat, double the, double alp)
        : longitude(lon), latitude(lat), theta(the), alpha(alp) {}
};

/**
 * 计算两车观测线交点（物体坐标）
 * 角度使用弧度
 * @param v1         车辆1信息
 * @param v2         车辆2信息
 * @param[out] obj_x 物体东向坐标
 * @param[out] obj_y 物体北向坐标
 * @return 是否计算成功（当观测线平行时返回false）
 */
bool calculateObjectPosition(const Vehicle &v1, const Vehicle &v2,
                             double &obj_x, double &obj_y);